/********** Local Includes ****************************************************/

#include "voronoi.h"
#include "objectdetector.h"
#include "common.h"
#include "imagesource.h"
#include "robot_orientation.h"

/********** Macros and Constants **********************************************/

#define BLUE cvScalar(255, 0, 0)
#define GREEN cvScalar(0, 255, 0)

/********** Namespaces ********************************************************/

using namespace cv;
using namespace std;

/********** Local Function Prototypes *****************************************/

/**
 * Matlab-style all() function.
 *
 * Returns true if all of the values in the matrix are non-zero, or false
 * otherwise.
 *
 * The given matrix should be type CV_32FC1.
 */
bool nonZero(const Mat &);

/**
 * Flips the 1's and 0's of a binary matrix
 *
 * Returns the flipped matrix
 *
 */
Mat flip( Mat matrix);

void showLarger(const Mat &image, int factor, char *name);

void show(const Mat &raw, string s);

/********** Public Methods ****************************************************/

Voronoi::Voronoi(ImageSource *source, Mat open) :
m_source(source),
m_open(open)
{
	if (!m_source) {
		ERROR("Image source cannot be null");
	}
	
	
}


Mat Voronoi::brushFire() {
	Mat edges(m_open.size(), CV_16UC1);
	Mat result(m_open.size(), CV_16UC1);
	
	result = Scalar(0);
	edges = Scalar(0);
	
	int height = result.size().height;
	int width = result.size().width;
	
	for (int row = 0; row < height; row++) {
		for (int col = 0; col < height; col++) {
			result.at<ushort>(row, col) = m_open.at<float>(row, col) < .5 ? 1 : 0;
		}
	}
	
	for (int row = 0; row < height; row++) {
		result.at<ushort>(row, 0) = 1;
		result.at<ushort>(row, width - 1) = 1;
	}
	
	for (int col = 0; col < width; col++) {
		result.at<ushort>(0, col) = 1;
		result.at<ushort>(height - 1, col) = 1;
	}
	
	Mat previous = result.clone();
	SHOW(result);
	waitKey(1);
	
	cout << "On iteration 0";
	
	for (int count = 1; !nonZero(result); count++) {
		for (int i = 0; i < 20; i++) {
			cout << "\b";
		}
		
		cout << "\rOn iteration " << count;
		
		for (int row = 0; row < height; row++) {
			for (int col = 0; col < width; col++) {
				if (result.at<ushort>(row, col) == count) {
					if (row > 0) {
						if (previous.at<ushort>(row - 1, col) == 0) {
							if (result.at<ushort>(row - 1, col) != 0) {
								edges.at<ushort>(row - 1, col) = 1;
							}
							
							result.at<ushort>(row - 1, col) = count + 1;
						}
					}

					if (row < height) {
						if (previous.at<ushort>(row + 1, col) == 0) {
							if (result.at<ushort>(row + 1, col) != 0) {
								edges.at<ushort>(row + 1, col) = 1;
							}
							
							result.at<ushort>(row + 1, col) = count + 1;
						}
					}

					if (col > 1) {
						if (previous.at<ushort>(row, col - 1) == 0) {
							if (result.at<ushort>(row, col - 1) != 0) {
								edges.at<ushort>(row, col - 1) = 1;
							}
							
							result.at<ushort>(row, col - 1) = count + 1;
						}
					}

					if (col < width) {
						if (previous.at<ushort>(row, col + 1) == 0) {
							if (result.at<ushort>(row, col + 1) != 0) {
								edges.at<ushort>(row, col + 1) = 1;
							}
							
							result.at<ushort>(row, col + 1) = count + 1;
						}
					}
				}
			}
		}

		previous = result.clone();
		//show(result, "result");
		
		waitKey(1);
		//while (waitKey(100) != 'c');
	}
	
	cout << endl;
	
	show(result, "result");
	return result;
	
	/*
	Mat edges2(edges.size(), CV_16UC1);
	edges2 = Scalar(0);
	
	for (int row = 0; row < height; row++) {
		for (int col = 0; col < width; col++) {
			vector<int> neighbors;
			neighbors.push_back(0);
			neighbors.push_back(0);
			neighbors.push_back(0);
			neighbors.push_back(0);
			
			int me = result.at<ushort>(row, col);
			
			if (row > 0) {
				neighbors[0] = result.at<ushort>(row - 1, col);
			}

			if (row < height) {
				neighbors[1] = result.at<ushort>(row + 1, col);
			}

			if (col > 0) {
				neighbors[2] = result.at<ushort>(row, col - 1);
			}

			if (col < width) {
				neighbors[3] = result.at<ushort>(row, col + 1);
			}
			
			if (neighbors[0] <= me && neighbors[1] <= me) {
				edges2.at<ushort>(row, col) = 1;
			}
			
			if (neighbors[2] <= me && neighbors[3] <= me) {
				edges2.at<ushort>(row, col) = 1;
			}
		}
	}

	Mat test(edges.size(), CV_16UC1);

	for (int row = 0; row < height; row++) {
		for (int col = 0; col < width; col++) {
			test.at<ushort>(row, col) = edges.at<ushort>(row, col)
				& edges2.at<ushort>(row, col);
		}
	}
	
	for (int row = 0; row < height - 1; row++) {
		for (int col = 0; col < width - 1; col++) {
		  int  val = result.at<ushort>(row, col);
		   
		   if (val > 1 && val == result.at<ushort>(row + 1, col) && val == result.at<ushort>(row, col + 1) && val == result.at<ushort>(row + 1, col + 1)) {
			   test.at<ushort>(row, col) = 1;
			   test.at<ushort>(row + 1, col) = 1;
			   test.at<ushort>(row, col + 1) = 1;
			   test.at<ushort>(row + 1, col + 1) = 1;
		   }
	   }
	}
	
	show(test, "test");
	return test;
	*/
}

int Voronoi::blockSize() const {
	return 20;
}


/********** Private Methods ***************************************************/



/********** Local Functions ***************************************************/

bool nonZero(const Mat &matrix) {
	
	for (int row = 0; row < matrix.size().height; row++) {
		for (int col = 0; col < matrix.size().width; col++) {
			if (matrix.at<float>(row, col) == 0) {
				return false;
			}
		}
	}
	return true;
}

Mat flip( Mat matrix) {
	for ( int row = 0; row < matrix.size().height; ++ row )
	{
		for ( int col = 0; col < matrix.size().width; ++ col )
		{
			if(matrix.at<float>(row, col) == 1){
				matrix.at<float>(row, col) =2;
			}
			if(matrix.at<float>(row, col) == 0){
				matrix.at<float>(row, col) =3;
			}
			
		}
	}
	
	
	for ( int row = 0; row < matrix.size().height; ++ row )
	{
		for ( int col = 0; col < matrix.size().width; ++ col )
		{
			if(matrix.at<float>(row, col) == 2){
				matrix.at<float>(row, col) =0;
			}
			if(matrix.at<float>(row, col) == 3){
				matrix.at<float>(row, col) =1;
			}
			
			
		}
	}
	
	return matrix;
}

void showLarger(const Mat &image, int scale, char *name) {
	Mat final(image.size() * scale, image.type());
	
	for (int row = 0; row < image.size().height; row++) {
		for (int col = 0; col < image.size().width; col++) {
			rectangle(final, Point(col, row) * scale,
				Point(col, row) * (scale + 1), image.at<float>(row, col),
				CV_FILLED);
		}
	}
	
	imshow(name, final);
}

void show(const Mat &raw, string s) {
	Mat image(raw.size(), CV_8UC3);
	double max = 0;
	
	for (int row = 0; row < raw.size().height; row++) {
		for (int col = 0; col < raw.size().width; col++) {
			if (raw.at<ushort>(row, col) > max) {
				max = raw.at<ushort>(row, col);
			}
		}
	}
	
	for (int row = 0; row < raw.size().height; row++) {
		for (int col = 0; col < raw.size().width; col++) {
			int value = int(raw.at<ushort>(row, col) / max * 255);
			image.at<Vec3b>(row, col)[0] = value;
			image.at<Vec3b>(row, col)[1] = value;
			image.at<Vec3b>(row, col)[2] = value;
		}
	}
	
	imshow(s, image);
}